#define _GNU_SOURCE

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <stdint.h>
#include <sys/ioctl.h>
#include <sys/socket.h>
#include <net/if.h>
#include <linux/can.h>
#include <linux/can/raw.h>
#include <pthread.h>
#include <getopt.h>
#include <errno.h>
#include <sys/select.h>

// 结构体：每个设备的统计信息
typedef struct {
    unsigned int lost;
    unsigned int dups;
    unsigned int bit_errors;
    unsigned int received;
} Stats;

// 结构体：接收线程参数
typedef struct {
    int socket;
    char* dev;
    unsigned int expected_id;
    unsigned int frames;
    volatile int* stop_flag;
    Stats* stats;
} RecvArg;

// 结构体：发送线程参数
typedef struct {
    int socket;
    unsigned int id;
    unsigned int frames;
    unsigned int delay_ms;
    unsigned int frames_cnt;
} SendArg;

// 全局停止标志（用于停止接收线程）
volatile int stop_receive = 0;

// 函数：设置CAN设备位定时（使用system调用ip link）
int set_can_bittiming(const char* dev, int bitrate, int tq, int prop_seg, int phase_seg1, int phase_seg2, int sjw) {
    char cmd[512] = {};

    // 先关闭设备
    sprintf(cmd, "ip link set %s down", dev);
    if (system(cmd) != 0) {
        fprintf(stderr, "错误: 无法关闭设备 %s\n", dev);
        return -1;
    }
    memset(cmd, 0, sizeof(cmd));

    // 如果提供了tq和所有段参数，计算比特率并校验
    if (tq > 0 && prop_seg >= 0 && phase_seg1 >= 0 && phase_seg2 >= 0 && sjw >= 0) {
        int num_quanta = 1 + prop_seg + phase_seg1 + phase_seg2;
        if (num_quanta <= 0) {
            fprintf(stderr, "错误: num_quanta 计算无效 (%d)\n", num_quanta);
            return -1;
        }
        long long bit_time_ns = (long long)tq * num_quanta;
        int computed_bitrate = (int)(1000000000LL / bit_time_ns);

        // 如果提供了bitrate，校验是否匹配
        if (bitrate > 0 && computed_bitrate != bitrate) {
            fprintf(stderr, "错误: 计算比特率 %d 与提供的 %d 不匹配\n", computed_bitrate, bitrate);
            return -1;
        }

        // 设置详细参数
        sprintf(cmd, "ip link set %s type can tq %d prop-seg %d phase-seg1 %d phase-seg2 %d sjw %d", 
                dev, tq, prop_seg, phase_seg1, phase_seg2, sjw);
    } else if (bitrate > 0) {
        // 仅设置bitrate
        sprintf(cmd, "ip link set %s type can bitrate %d", dev, bitrate);
    } else {
        fprintf(stderr, "错误: 缺少必要的位定时参数\n");
        return -1;
    }

    if (system(cmd) != 0) {
        fprintf(stderr, "错误: 设置 %s 位定时失败\n", dev);
        return -1;
    }

    // 打开设备
    memset(cmd, 0, sizeof(cmd));
    sprintf(cmd, "ip link set %s up", dev);
    if (system(cmd) != 0) {
        fprintf(stderr, "错误: 无法打开设备 %s\n", dev);
        return -1;
    }

    return 0;
}

// 函数：创建CAN socket并绑定
int create_can_socket(const char* dev, unsigned int id, int sndbuf_size, int rcvbuf_size) {
    int s = socket(PF_CAN, SOCK_RAW, CAN_RAW);
    if (s < 0) {
        perror("socket");
        return -1;
    }

    struct ifreq ifr;
    strcpy(ifr.ifr_name, dev);
    if (ioctl(s, SIOCGIFINDEX, &ifr) < 0) {
        perror("ioctl SIOCGIFINDEX");
        close(s);
        return -1;
    }

    struct sockaddr_can addr;
    addr.can_family = AF_CAN;
    addr.can_ifindex = ifr.ifr_ifindex;
    if (bind(s, (struct sockaddr*)&addr, sizeof(addr)) < 0) {
        perror("bind");
        close(s);
        return -1;
    }

    // 禁用本地回环
    int loopback = 0;
    setsockopt(s, SOL_CAN_RAW, CAN_RAW_LOOPBACK, &loopback, sizeof(loopback));

    // 设置过滤器，只接收指定ID
    struct can_filter filter[1];
    filter[0].can_id = id;
    filter[0].can_mask = CAN_SFF_MASK;
    setsockopt(s, SOL_CAN_RAW, CAN_RAW_FILTER, filter, sizeof(filter));

    // 设置发送缓冲区大小 (SO_SNDBUF)
    if (setsockopt(s, SOL_SOCKET, SO_SNDBUF, &sndbuf_size, sizeof(sndbuf_size))) {
        perror("setsocketopt 设置发送缓冲区失败");
        close(s);
        return -1;
    }

    // 设置接收缓冲区大小 (SO_RCVBUF)
    if (setsockopt(s, SOL_SOCKET, SO_RCVBUF, &rcvbuf_size, sizeof(rcvbuf_size))) {
        perror("setsocketopt 设置发送缓冲区失败");
        close(s);
        return -1;
    }

    return s;
}

// 接收线程函数：接收并校验数据
void* receive_thread(void* arg) {
    RecvArg* rarg = (RecvArg*)arg;
    int s = rarg->socket;
    unsigned int expected_id = rarg->expected_id;
    unsigned int frames = rarg->frames;
    volatile int* stop = rarg->stop_flag;
    Stats* stats = rarg->stats;

    unsigned int expected_seq = 0;
    stats->lost = 0;
    stats->dups = 0;
    stats->bit_errors = 0;
    stats->received = 0;

    while (!(*stop)) {
        struct timeval tv = {0, 100000}; // 超时100ms，用于检查stop标志
        fd_set fds;
        FD_ZERO(&fds);
        FD_SET(s, &fds);

        int ret = select(s + 1, &fds, NULL, NULL, &tv);
        if (ret > 0) {
            struct can_frame frame;
            if (read(s, &frame, sizeof(frame)) < 0) {
                perror("read");
                continue;
            }

            if (frame.can_id != expected_id || frame.can_dlc != 8) continue;

            uint32_t seq = *(uint32_t*)(frame.data);
            uint32_t check = *(uint32_t*)(frame.data + 4);

            // 校验位错误
            if (check != ~seq) {
                stats->bit_errors++;
                // printf("[%s] 位错误: seq %u, 预期 ~seq = 0x%x, 实际 0x%x\n", 
                //        rarg->dev, seq, ~seq, check);
                printf("[%s] 位错误: 预期数据 [0x%08x, 0x%08x], 实际数据 [0x%08x, 0x%08x]\n", 
                        rarg->dev, expected_seq, ~expected_seq, seq, check);
                continue;
            }

            // 序列号校验
            if (seq == expected_seq) {
                expected_seq++;
                stats->received++;
            } else if (seq > expected_seq) {
                // 检测丢帧，打印每个丢失帧的预期数据，然后重新同步
                unsigned int lost_count = seq - expected_seq;
                stats->lost += lost_count;
                for (unsigned int i = expected_seq; i < seq; i++) {
                    printf("[%s] 丢帧: seq %u, 预期数据 [0x%08x, 0x%08x] (前4字节seq, 后4字节~seq), 实际无数据\n", 
                           rarg->dev, i, i, ~i);
                }
                expected_seq = seq + 1;
                stats->received++;
            } else {
                // 重复帧
                stats->dups++;
                printf("[%s] 重复帧: seq %u, 实际数据 [0x%08x, 0x%08x], 预期无（已接收）\n", 
                       rarg->dev, seq, seq, check);
            }
        } else if (ret < 0) {
            perror("select");
        }
    }

    // 停止后，检查是否丢失尾部帧
    if (expected_seq < frames) {
        unsigned int additional_lost = frames - expected_seq;
        stats->lost += additional_lost;
        for (unsigned int i = expected_seq; i < frames; i++) {
            printf("[%s] 尾部丢帧: seq %u, 预期数据 [0x%08x, 0x%08x], 实际无数据\n", 
                   rarg->dev, i, i, ~i);
        }
    }

    return NULL;
}

// 发送线程函数：发送指定帧数
void* send_thread(void* arg) {
    SendArg* sarg = (SendArg*)arg;
    int s = sarg->socket;
    unsigned int id = sarg->id;
    unsigned int frames = sarg->frames;
    unsigned int delay_ms = sarg->delay_ms;

    for (sarg->frames_cnt = 0; sarg->frames_cnt < frames; sarg->frames_cnt++) {
        struct can_frame frame;
        frame.can_id = id;
        frame.can_dlc = 8;
        *(uint32_t*)(frame.data) = sarg->frames_cnt;
        *(uint32_t*)(frame.data + 4) = ~sarg->frames_cnt;

        if (write(s, &frame, sizeof(frame)) != sizeof(frame)) {
            perror("write");
        }

        usleep(delay_ms * 1000);
    }

    return NULL;
}

int main(int argc, char** argv) {
    // 默认值
    char dev1[64] = {};
    char dev2[64] = {};
    unsigned int id1 = 1;
    unsigned int id2 = 2;
    unsigned int frames = 100;
    unsigned int delay_ms = 100;
    int bitrate = 250000;
    int tq = 160;
    int prop_seg = 8;
    int phase_seg1 = 8;
    int phase_seg2 = 8;
    int sjw = 4;
    int stop = 0;
    int sndbuf_size = 1024 * 1024;
    int rcvbuf_size = 1024 * 1024;

    // 解析命令行参数
    struct option long_options[] = {
        {"dev1", required_argument, NULL, 'a'},
        {"dev2", required_argument, NULL, 'b'},
        {"id1", required_argument, NULL, 'c'},
        {"id2", required_argument, NULL, 'd'},
        {"frames", required_argument, NULL, 'e'},
        {"delay", required_argument, NULL, 'f'},
        {"bitrate", required_argument, NULL, 'g'},
        {"tq", required_argument, NULL, 'h'},
        {"prop-seg", required_argument, NULL, 'i'},
        {"phase-seg1", required_argument, NULL, 'j'},
        {"phase-seg2", required_argument, NULL, 'k'},
        {"sjw", required_argument, NULL, 'l'},
        {"stop", no_argument, NULL, 'm'},
        {"sndbuf", required_argument, NULL, 'n'},
        {"rcvbuf", required_argument, NULL, 'o'},
        {NULL, 0, NULL, 0}
    };

    int opt;
    while ((opt = getopt_long(argc, argv, "", long_options, NULL)) != -1) {
        switch (opt) {
            case 'a':
                strcpy(dev1, optarg);
                break;
            case 'b': 
                strcpy(dev2, optarg);
                break;
            case 'c': id1 = strtoul(optarg, NULL, 0); break;
            case 'd': id2 = strtoul(optarg, NULL, 0); break;
            case 'e': frames = atoi(optarg); break;
            case 'f': delay_ms = atoi(optarg); break;
            case 'g': bitrate = atoi(optarg); break;
            case 'h': tq = atoi(optarg); break;
            case 'i': prop_seg = atoi(optarg); break;
            case 'j': phase_seg1 = atoi(optarg); break;
            case 'k': phase_seg2 = atoi(optarg); break;
            case 'l': sjw = atoi(optarg); break;
            case 'm': stop = 1; break;
            case 'n': sndbuf_size = strtoul(optarg, NULL, 0); break;
            case 'o': rcvbuf_size = strtoul(optarg, NULL, 0); break;
            default: 
                fprintf(stderr, "\n    用法: %s --dev1 can0 --dev2 can1 [其他参数]\n", argv[0]); 
                fprintf(stderr, "\n    例子: %s --dev1 can0 --dev2 can1 --id1 1 --id2 2 --frames 100 --delay 100 --bitrate 250000 --tq 160 --prop-seg 8 --phase-seg1 8 --phase-seg2 8 --sjw 4\n\n", argv[0]);
                exit(1);
        }
    }

    if (!(*dev1) || !(*dev2)) {
        fprintf(stderr, "错误: 必须提供 --dev1, --dev2\n");

        fprintf(stderr, "\n    用法: %s --dev1 can0 --dev2 can1 [其他参数]\n", argv[0]); 
        fprintf(stderr, "           [其他参数]:\n"
                        "                    --id1 --id2: 选择互相回环的can设备\n"
                        "                    --frames : 每次循环发送多少帧\n"
                        "                    --delay  : 每帧发送延时(ms)\n"
                        "                    --bitrate: 波特率\n"
                        "                    --tq --prop-seg --phase-seg1 --phase-seg2 --sjw : can设备基本参数\n"
                        "                    --sndbuf --rcvbuf: socket can buf大小设置\n"
                        "                    --stop: 出错停止/继续 (无参数)\n");
        fprintf(stderr, "\n    例子: %s --dev1 can0 --dev2 can1 --id1 1 --id2 2 --frames 100 --delay 100 --bitrate 250000 --tq 160 --prop-seg 8 --phase-seg1 8 --phase-seg2 8 --sjw 4\n\n", argv[0]);
        exit(1);
    }

    unsigned int total_cycle_errors = 0;
    // 无限循环：每个周期进行测试
    int cycle = 0;
    while (1) {
        cycle++;
        printf("\n周期 %d 开始\n", cycle);

        printf("设备1: %s, 设备1: %s\n", dev1, dev2);
        printf("sndbuf 缓冲区大小: %d, rcvbuf 缓冲区大小: %d\n", sndbuf_size, rcvbuf_size);
        printf("%s 发送 can id: 0x%x(%d), %s 发送 can id: 0x%x(%d)\n", dev1, id1, id1, dev2, id2, id2);
        printf("每一轮发送 %d 帧，每帧间隔 %d ms (出错后%s)\n", frames, delay_ms, stop ? "停止": "继续运行");
        printf("波特率： %d, tq: %d, prop-seg: %d, phase-seg1: %d, phase-seg2: %d, sjw: %d\n", bitrate, tq, prop_seg, phase_seg1, phase_seg2, sjw);



        // 设置位定时
        if (set_can_bittiming(dev1, bitrate, tq, prop_seg, phase_seg1, phase_seg2, sjw) < 0 ||
            set_can_bittiming(dev2, bitrate, tq, prop_seg, phase_seg1, phase_seg2, sjw) < 0) {
            exit(1);
        }

        // 创建socket
        int s1 = create_can_socket(dev1, id1, sndbuf_size, rcvbuf_size);
        int s2 = create_can_socket(dev2, id2, sndbuf_size, rcvbuf_size);
        if (s1 < 0 || s2 < 0) exit(1);

        // 重置停止标志
        stop_receive = 0;

        // 准备统计
        Stats stats1 = {0}, stats2 = {0};

        // 创建接收线程（先启动）
        pthread_t recv_t1, recv_t2;
        RecvArg rarg1 = {s1, dev1, id1, frames, &stop_receive, &stats1};
        RecvArg rarg2 = {s2, dev2, id2, frames, &stop_receive, &stats2};
        pthread_create(&recv_t1, NULL, receive_thread, &rarg1);
        pthread_create(&recv_t2, NULL, receive_thread, &rarg2);

        // 创建发送线程
        pthread_t send_t1, send_t2;
        SendArg sarg1 = {s1, id2, frames, delay_ms};
        SendArg sarg2 = {s2, id1, frames, delay_ms};
        pthread_create(&send_t1, NULL, send_thread, &sarg1);
        pthread_create(&send_t2, NULL, send_thread, &sarg2);

        // 等待发送完成
        // pthread_join(send_t1, NULL);
        // pthread_join(send_t2, NULL);
        int pth_ret = 0, pth_run1 = 0, pth_run2 = 0;
        while(1) {
            usleep(1000);
            pth_ret = pthread_tryjoin_np(send_t1, NULL);
            if (0 == pth_ret) {
                // printf("%s 发送已完成\n", dev1);
                pth_run1 = 1;
            } else if (EBUSY == pth_ret) {
            } else {
                printf("%s 其他错误\n", dev1);
            }
            pth_ret = pthread_tryjoin_np(send_t2, NULL);
            if (0 == pth_ret) {
                // printf("%s 发送已完成\n", dev2);
                pth_run2 = 1;
            } else if (EBUSY == pth_ret) {
            } else {
                printf("%s 其他错误\n", dev2);
            }
            printf("\r 正在发送: %s --> %s %d 帧, %s --> %s %d 帧", dev1, dev2, sarg1.frames_cnt, dev2, dev1, sarg2.frames_cnt);
            if (1 == pth_run1 && 1 == pth_run2) {
                printf("\n");
                break;
            }
        }


        // 休眠 100 ms，保证接收完成
        usleep(1000 * 100);

        // 停止接收
        stop_receive = 1;
        pthread_join(recv_t1, NULL);
        pthread_join(recv_t2, NULL);

        // 打印统计
        printf("[%s] 统计: 接收 %u, 丢帧 %u, 重复 %u, 位错误 %u\n", dev1, stats1.received, stats1.lost, stats1.dups, stats1.bit_errors);
        printf("[%s] 统计: 接收 %u, 丢帧 %u, 重复 %u, 位错误 %u\n", dev2, stats2.received, stats2.lost, stats2.dups, stats2.bit_errors);

        // 判断是否成功
        unsigned int total_errors = stats1.lost + stats1.dups + stats1.bit_errors + stats2.lost + stats2.dups + stats2.bit_errors;
        total_cycle_errors += total_errors;
        if (total_errors == 0) {
            printf("周期 %d 校验成功\n", cycle);
        } else {
            printf("周期 %d 校验失败, 当前循环错误计数 %u\n", cycle, total_errors);
            if (stop) {
                break;
            }
        }
        printf("所有循环 总错误计数: %u\n", total_cycle_errors);

        // 关闭设备
        close(s1);
        close(s2);
        char cmd[100] = {};
        // sprintf(cmd, "ip -d link show %s", dev1); system(cmd);
        // memset(cmd, 0, sizeof(cmd));
        // sprintf(cmd, "ip -d link show %s", dev2); system(cmd);
        // memset(cmd, 0, sizeof(cmd));

        sprintf(cmd, "ip link set %s down", dev1); system(cmd);
        memset(cmd, 0, sizeof(cmd));
        sprintf(cmd, "ip link set %s down", dev2); system(cmd);

        // 每个周期间隔1s（可调整）
        // sleep(1);
    }

    return 0;
}
